﻿#include "AutoFocus1.h"
#include <QDebug>
#include <QElapsedTimer>
//#include "libswscale/swscale.h"
//#include "libavutil/pixfmt.h"

AutoFocus1::AutoFocus1(QObject *parent)
    :QObject(parent)
{
    m_focusMeasure.setZoomFactor(2);
    m_pWatchDog = new QTimer(this);
    m_pWatchDog->setSingleShot(true);
    connect(m_pWatchDog, &QTimer::timeout, this, &AutoFocus1::watchdogTimeout);
}

void AutoFocus1::stop()
{
    m_pWatchDog->stop();
    m_pStageControl->stopZ();
}

void AutoFocus1::zMoveFinished()
{
    switch (m_state) {
    case WAIT_MOVEUP_FINISH: //向上运动已经完成了，开始向下运动
        qDebug() << "WAIT_MOVEUP_FINISH";
        stopWatchDog();
        m_state = WAIT_MOVEDOWN_FINISH;
        m_pStageControl->zMoveToPos(m_z_mm + m_low_mm, m_speed);
        m_count = 0;
        startWatchDog();
        break;
    case WAIT_MOVEDOWN_FINISH:
        qDebug() << "WAIT_MOVEDOWN_FINISH";
        stopWatchDog();
        m_state = WAIT_STOP;
        if(m_pos >= 0 && m_pos <= m_count)
        {
            double z = m_z_mm + m_high_mm + (m_low_mm - m_high_mm) / m_count * m_pos;
            m_pStageControl->zMoveToPos(z, m_speed);
            qDebug() << "WAIT_MOVEDOWN_FINISH  zMoveToPos  m_count = "  << m_count << ", m_pos = " << m_pos;
        }
        else
        {
            m_pStageControl->zMoveToPos(m_z_mm, m_speed);
        }

        startWatchDog();
        break;
    case WAIT_STOP:
        qDebug() << "WAIT_STOP, autoFocusFinished";
        stopWatchDog();
        m_state = IDLE;
        emit autoFocusFinished();
        break;
    default: //这时候什么都不做
        break;
    }
}

void AutoFocus1::watchdogTimeout()
{
    qDebug() << "watchdogTimeout()";
    double m_theoryPos_mm[4];
    double m_encodePos_mm[4];
    m_pStageControl->updatePos(m_theoryPos_mm, m_encodePos_mm);
    double z_mm = m_theoryPos_mm[2];
    double target_z_mm = 0.0;
    switch (m_state) {
    case WAIT_MOVEUP_FINISH:
        m_state = WAIT_MOVEDOWN_FINISH;
        target_z_mm = m_z_mm + m_high_mm;
        if(!m_pStageControl->isMoving(AXIS_Z) && fabs(z_mm - target_z_mm) < 0.01) //说明运动到位了
        {
            m_pStageControl->zMoveToPos(m_z_mm + m_low_mm, m_speed);
            startWatchDog();
        }
        break;
    case WAIT_MOVEDOWN_FINISH:
        m_state = WAIT_STOP;
        target_z_mm = m_z_mm + m_high_mm;
        if(!m_pStageControl->isMoving(AXIS_Z) && fabs(z_mm - target_z_mm) < 0.01) //说明运动到位了
        {
            m_pStageControl->zMoveToPos(m_z_mm, m_speed);
            startWatchDog();
        }
        break;
    case WAIT_STOP:
        m_state = IDLE;
        //qDebug() << "autoFocus Finished";
        emit autoFocusFinished();
        break;
    default: //这时候什么都不做
        break;
    }
}
void AutoFocus1::startWatchDog(int ms)
{
    if(ms <= 0)
    {
        m_pWatchDog->start(10000);
        return;
    }
    m_pWatchDog->start(ms);
}

void AutoFocus1::stopWatchDog()
{
    m_pWatchDog->stop();
}

void AutoFocus1::startAutoFocus()
{
    m_bestScore = 0.0;
    m_count = 0;
    if(m_high_mm == 0.0 && m_low_mm == 0.0)
    {
        //这时不用做自动对焦。当前图像就是最佳图像
        QImage image = m_pCameraImage->getImage();
        setImage(image);
        qDebug() << "autoFocus Finished";
        emit autoFocusFinished();
        return;
    }
    double m_theoryPos_mm[4];
    double m_encodePos_mm[4];
    m_pStageControl->updatePos(m_theoryPos_mm, m_encodePos_mm);

    qDebug()<< "z pos = " << m_theoryPos_mm[2];
    m_z_mm = m_theoryPos_mm[2];
//    m_pCamera->startGrabbing(); // 必须保证相机是采图模式
    m_state = WAIT_MOVEUP_FINISH; // 先向上运动
    m_pStageControl->zMoveToPos(m_z_mm + m_high_mm, 1.0);
    startWatchDog();
}

void AutoFocus1::attach(Qly::CameraImage *image)
{
    assert(image);
    m_pCameraImage = image;
    connect(m_pCameraImage, &Qly::CameraImage::imageChanged, this, &AutoFocus1::setImage);
}

void AutoFocus1::attach(StageControl *control)
{
    assert(control);
    m_pStageControl = control;
    connect(m_pStageControl, &StageControl::zMoveFinish, this, &AutoFocus1::zMoveFinished);

}

double AutoFocus1::bestImage(QImage &image)
{
    image = m_bestImage;
    return m_bestScore;
}

void AutoFocus1::setRange(double lower_mm, double higher_mm, double speed)
{
    m_low_mm = lower_mm;
    m_high_mm = higher_mm;
    m_speed = speed;
}

void AutoFocus1::attach(Qly::IndustryCamera * camera)
{
    assert(camera);
    m_pCamera = camera;
}

//void toGray(const QImage &image, cv::Mat &gray, int scale)
//{
//    int srcW = image.width();
//    int srcH = image.height();
//    int desW = srcW / scale;
//    int desH = srcH / scale;

//    AVPixelFormat srcFormat = AV_PIX_FMT_RGB24;

//    uint8_t *in_data[1];
//    int in_linesize[1];
//    in_data[0] = (uint8_t *) image.bits();
//    in_linesize[0] = image.bytesPerLine();
//    //av_image_fill_arrays(in_data, in_linesize, inImage.bits(), AV_PIX_FMT_YUYV422, srcW, srcH, 1);

//    uint8_t *out_data[1];
//    int out_linesize[1];
//    out_data[0] = gray.ptr<uchar>(0);
//    out_linesize[0] = gray.step ;

//    SwsContext * pContext = sws_getContext(srcW, srcH, srcFormat,
//                                           desW, desH, srcFormat, SWS_FAST_BILINEAR, nullptr, nullptr, nullptr);
//    if(!pContext) return;
//    sws_scale(pContext, in_data, in_linesize, 0, srcH,
//              out_data, out_linesize);
//    sws_freeContext(pContext);
//}

void AutoFocus1::setImage(QImage &RGB24Image)
{
    static int i = 0;
    static cv::Mat gray;
//    static QElapsedTimer t;
    if(m_state != IDLE)// && i % 3 == 1)
    {
//        t.start();
//        int w = RGB24Image.width();
//        int h = RGB24Image.height();
//        gray.create(h / 2, w / 2, CV_8UC1);
//        toGray(RGB24Image, gray, 2);
        //QImage temp = RGB24Image.copy();
        //qDebug() << "image is null :" << temp.isNull();
        cv::Mat mat = m_focusMeasure.preprocess(RGB24Image);
        double score = m_focusMeasure.measure(mat);
        m_count ++;
//        double score = m_focusMeasure.measure(gray);
        if(score > m_bestScore)
        {
            m_bestScore = score;
            m_pos = m_count; // 记录位置
            m_bestImage = RGB24Image;
        }
//        qDebug() << "time = " << t.elapsed();
    }
    i ++;
}
